#include "radiation_source_state_machine/lifecycle_node_client.hpp"
#include <chrono>
#include <thread>

namespace radiation_source_state_machine
{

LifecycleNodeClient::LifecycleNodeClient(rclcpp::Node::SharedPtr parent_node,
    const std::string& node_name)
    : parent_node_(parent_node),
      node_name_(node_name),
      is_client_initialized_(false)
{
    std::string change_state_service = "/" + node_name_ + "/change_state";
    std::string get_state_service = "/" + node_name_ + "/get_state";

    try {
        change_state_client_ = parent_node_->create_client<lifecycle_msgs::srv::ChangeState>(
            change_state_service);
        get_state_client_ = parent_node_->create_client<lifecycle_msgs::srv::GetState>(
            get_state_service);
        
        if (!change_state_client_->wait_for_service(std::chrono::seconds(2)) ||
            !get_state_client_->wait_for_service(std::chrono::seconds(2))) {
            RCLCPP_ERROR(parent_node_->get_logger(), 
                        "Services for node %s not available after 2 seconds", node_name_.c_str());
            is_client_initialized_ = false;
            return;
        }

        current_state_.id = lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN;
        current_state_.label = "unknown";
        is_client_initialized_ = true;
        RCLCPP_INFO(parent_node_->get_logger(), 
                   "Successfully initialized lifecycle client for node: %s", node_name_.c_str());
    } catch (const std::exception& e) {
        RCLCPP_ERROR(parent_node_->get_logger(), 
                    "Failed to initialize lifecycle client for node %s: %s", 
                    node_name_.c_str(), e.what());
        is_client_initialized_ = false;
    }
}

bool LifecycleNodeClient::configure()
{
    if (!is_client_initialized_) return false;

    auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>();
    request->transition.id = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;

    auto future = change_state_client_->async_send_request(request);
    if (rclcpp::spin_until_future_complete(parent_node_, future, std::chrono::seconds(3)) !=
        rclcpp::FutureReturnCode::SUCCESS) {
        RCLCPP_ERROR(parent_node_->get_logger(), 
                    "Failed to configure node: %s - timeout after 3 seconds", node_name_.c_str());
        return false;
    }

    auto result = future.get();
    if (!result->success) {
        RCLCPP_ERROR(parent_node_->get_logger(), 
                    "Configuration failed for node: %s", node_name_.c_str());
        return false;
    }

    RCLCPP_INFO(parent_node_->get_logger(), 
               "Successfully configured node: %s", node_name_.c_str());
    return true;
}

bool LifecycleNodeClient::activate()
{
    if (!is_client_initialized_) return false;

    auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>();
    request->transition.id = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE;

    auto future = change_state_client_->async_send_request(request);
    if (rclcpp::spin_until_future_complete(parent_node_, future, std::chrono::seconds(3)) !=
        rclcpp::FutureReturnCode::SUCCESS) {
        RCLCPP_ERROR(parent_node_->get_logger(), 
                    "Failed to activate node: %s", node_name_.c_str());
        return false;
    }

    auto result = future.get();
    if (!result->success) {
        RCLCPP_ERROR(parent_node_->get_logger(), 
                    "Activation failed for node: %s", node_name_.c_str());
        return false;
    }

    RCLCPP_INFO(parent_node_->get_logger(), 
               "Successfully activated node: %s", node_name_.c_str());
    return true;
}

bool LifecycleNodeClient::deactivate()
{
    if (!is_client_initialized_) return false;

    auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>();
    request->transition.id = lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE;

    auto future = change_state_client_->async_send_request(request);
    if (rclcpp::spin_until_future_complete(parent_node_, future, std::chrono::seconds(3)) !=
        rclcpp::FutureReturnCode::SUCCESS) {
        RCLCPP_ERROR(parent_node_->get_logger(), 
                    "Failed to deactivate node: %s", node_name_.c_str());
        return false;
    }

    auto result = future.get();
    if (!result->success) {
        RCLCPP_ERROR(parent_node_->get_logger(), 
                    "Deactivation failed for node: %s", node_name_.c_str());
        return false;
    }

    RCLCPP_INFO(parent_node_->get_logger(), 
               "Successfully deactivated node: %s", node_name_.c_str());
    return true;
}

bool LifecycleNodeClient::cleanup()
{
    if (!is_client_initialized_) return false;

    auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>();
    request->transition.id = lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP;

    auto future = change_state_client_->async_send_request(request);
    if (rclcpp::spin_until_future_complete(parent_node_, future, std::chrono::seconds(3)) !=
        rclcpp::FutureReturnCode::SUCCESS) {
        RCLCPP_ERROR(parent_node_->get_logger(), 
                    "Failed to cleanup node: %s - timeout after 3 seconds", node_name_.c_str());
        return false;
    }

    auto result = future.get();
    if (!result->success) {
        RCLCPP_ERROR(parent_node_->get_logger(), 
                    "Cleanup failed for node: %s", node_name_.c_str());
        return false;
    }

    RCLCPP_INFO(parent_node_->get_logger(), 
               "Successfully cleaned up node: %s", node_name_.c_str());
    return true;
}


lifecycle_msgs::msg::State LifecycleNodeClient::get_current_state()
{
    if (!is_client_initialized_) {
        lifecycle_msgs::msg::State state;
        state.id = lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN;
        state.label = "uninitialized";
        return state;
    }

    auto request = std::make_shared<lifecycle_msgs::srv::GetState::Request>();
    auto future = get_state_client_->async_send_request(request);
    
    if (rclcpp::spin_until_future_complete(parent_node_, future, std::chrono::seconds(2)) !=
        rclcpp::FutureReturnCode::SUCCESS) {
        RCLCPP_WARN(parent_node_->get_logger(), 
                   "Failed to get state for node: %s - timeout after 2 seconds", node_name_.c_str());
        current_state_.id = lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN;
        current_state_.label = "unknown";
        return current_state_;
    }

    auto result = future.get();
    current_state_ = result->current_state;
    return current_state_;
}

bool LifecycleNodeClient::wait_for_state(
    lifecycle_msgs::msg::State::LifecycleState target_state,
    std::chrono::milliseconds timeout)
{
    auto start_time = std::chrono::steady_clock::now();
    auto end_time = start_time + timeout;

    while (std::chrono::steady_clock::now() < end_time) {
        auto current_state = get_current_state();
        if (current_state.id == target_state) {
            return true;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    RCLCPP_ERROR(parent_node_->get_logger(), 
                "Timeout waiting for state %d on node %s after %ld ms", 
                target_state, node_name_.c_str(), timeout.count());
    return false;
}

bool LifecycleNodeClient::is_active() const
{
    return is_client_initialized_ && 
           current_state_.id == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
}

}  // namespace radiation_source_state_machine